
#include "LSM303DLLinear.h"
#include "..\System\F2806x_Device.h"
#include <math.h>

bool LSM303DLLinear::Init()
{
	// init I2C Driver
	m_SAR = 0x0019; // LSM303DLH - Linear I2C address
	InitI2C();
	
	// check Ctrlreg1 (no WhoAmI regs on this crap!)
	Uint16 WhoAMI = 0x00;
	ReadReg16(0x20, &WhoAMI);
	
	// this doesn't work if LSM303 is not reset
	//if( WhoAMI != 0x0707 ) // 16 bits read - same register read twice! 
	//{
	//	// linear failed
	//	return false;
	//}
	
	// set ctrl reg1
	Uint16 ctrl1;
	ctrl1 = 0x27; // XYZ enabled, ODR: 50Hz (37Hz cut-off), Normal Mode
	//ctrl1 = 0x2F; // XYZ enabled, ODR: 100Hz (74Hz cut-off, Normal Mode
	//ctrl1 = 0x37; // XYZ enabled, ODR: 400Hz (292Hz cut-off), Normal Mode
	WriteReg8(0x20, ctrl1);
	
	// set ctrl reg4
	Uint16 ctrl4;
	ctrl4 = 0x80; // BDU = 1!, +-2g, self test disabled
	WriteReg8(0x23, ctrl4);
	
	return true;
}

void LSM303DLLinear::Process()
{
	int16 axes[3];
	axes[0] = 0;
	axes[1] = 0;
	axes[2] = 0;
	ReadReg48Swapped(0x28 + 0x80, (Uint16*)axes);
		
	m_AxisX = axes[0]/16.0f * 1e-3f; // +-2g -> 1.0 mg/digit
	m_AxisY = axes[1]/16.0f * 1e-3f;
	m_AxisZ = axes[2]/16.0f * 1e-3f;
}
